#include <math.h>
#include <common/miscutils.h>
#include "properties.h"
#include "watcher.h"

Watcher::Watcher() {}

Watcher::~Watcher() {}

bool Watcher::isOk() const {
	PropAccessor p;

	switch (p().fcs.watcher) {
	case Watcher_Idle: return true;
	case Watcher_Rolling: return isRollingOk();
	case Watcher_GlidePath: return isGlidePathOk();
	}

	return true;
}

bool Watcher::isRollingOk() const {
	const float maxXte = 10;
	const float maxCourseError = 45;
	const float minGroundSpeed = 3;

	PropAccessor p;
	return (fabs(p().fcs.track.xte) < maxXte && fabs(p().fcs.track.courseError) < maxCourseError) 
		|| (p().position.ground_speed < minGroundSpeed);
}

bool Watcher::isGlidePathOk() const {
	PropAccessor p;

	float atd = max(p().fcs.track.atd, 0.0f);
	float maxXte = 5 + atd * 0.05f;
	float maxAltError = 3 + atd * 0.02f;

	return fabs(p().fcs.track.xte) < maxXte && fabs(p().fcs.track.altError) < maxAltError;
}
